#include "matplotlibcpp.hpp"
#include "yaml_to_2d_table.h"
#include "test_utilities_path.h"
#include <cmath>
using auto_ros::common_tool::Interpolation2D;
using auto_ros::control::yaml_to_2d_table;
namespace plt = matplotlibcpp;

int main()
{
	//bool yaml_to_2d_table(std::string engine_map_yaml_path_file, Interpolation2D &map_2d)
	Interpolation2D map_2d;
	if (false == yaml_to_2d_table(test_utilities_path + "map_test.yaml", map_2d))
	{
		return 0;
	}

	map_2d.Interpolate(std::make_pair(1.6, 0.1));
	std::vector<std::vector<double>> x, y, z;
	for (double i = -5; i <= 5; i += 0.25)
	{
		std::vector<double> x_row, y_row, z_row;
		for (double j = -5; j <= 5; j += 0.25)
		{
			x_row.push_back(i);
			y_row.push_back(j);
			z_row.push_back(::std::sin(::std::hypot(i, j)));
		}
		x.push_back(x_row);
		y.push_back(y_row);
		z.push_back(z_row);
	}

	plt::plot_surface(x, y, z);
	plt::show();
}